﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;

namespace DotNetGPSTest
{
   class NewLatLongToOSGB
   {
      private const double scalechange = (20.4894 / 1000000.0);
       private const double rz = (-0.8421);
       private const double ry = (-0.247);
       private const double rx = (-0.1502);

       private const double r0c0 = (1 + scalechange);
       private const double r0c1 = (-1 * rz * 0.000004848136811);
       private const double r0c2 = (ry * 0.000004848136811);

       private const double r1c0 = (rz * 0.000004848136811);
       private const double r1c1 = r0c0;
       private const double r1c2 = (-1 * rx * 0.000004848136811);

       private const double r2c0 = (-1 * ry * 0.000004848136811);
       private const double r2c1 = (rx * 0.000004848136811);
       private const double r2c2 = r0c0;

       private const double pi = 3.1415926535;
       private const double NGa = 6377563.396;
       private const double NGb = 6356256.910;
       private const double WGS84a = 6378137.0;
       private const double WGS84b = 6356752.3141;

       private const double NGFo = 0.9996012717;
       private const double NGphiOrigin = (49.0 * pi / 180);
       private const double NGlamdaOrigin = (-2.0 * pi / 180);
       private const double NGeOrigin = 400000.0;
       private const double NGnOrigin = -100000.0;

       private const double m_aSquared = (NGa * NGa);
       private const double m_bSquared = (NGb * NGb);
       private const double m_eSquared = ((m_aSquared - m_bSquared) / m_aSquared);
       private const double m_n = ((NGa - NGb) / (NGa + NGb));
       private const double m_nToTheTwo = (m_n * m_n);
       private const double m_nToTheThree = (m_nToTheTwo * m_n);
       private const double m_NGaNGFo = (NGa * NGFo);
       private const double m_NGbNGFo = (NGb * NGFo);
       private const double m_WGS84aSquared = (WGS84a * WGS84a);
       private const double m_WGS84bSquared = (WGS84b * WGS84b);
       private const double m_WGS84eSquared = ((m_WGS84aSquared - m_WGS84bSquared) / m_WGS84aSquared);

       private double m_Northing;
       private double m_Easting;

       public double lati_;
       public double longi_;
       public string result_;
       public int e;
       public int n;

       //public NewLatLongToOSGB()
       //{
       //}

       private void InputValidation()
       {
          // some validation
          if (lati_ < 30.0)
             throw new FormatException("Too far South!");

          if (lati_ > 70.0)
             throw new FormatException("Too far North!");

          if (longi_ < -10.0)
             throw new FormatException("Too far West!");

          if (longi_ > 3.0)
             throw new FormatException("Too far East!");
       }

       private void ValidateOSGB()
       {
          // some validation
          if (m_Northing < 0.0)
             throw new FormatException("Too far South!");

          if (m_Northing > 1300000.0)
             throw new FormatException("Too far North!");

          if (m_Easting < 0.0)
             throw new FormatException("Too far West!");

          if (m_Easting > 700000.0)
             throw new FormatException("Too far East!");
       }

       private void HelTrans(ref double phi, ref double lamda, ref double ElipsoidHeight)
       {
          // Helmert Transformation

          // lat long height

          //This lat long to 3-d uses WGS84 m_eSquaredIN, NGaIN

          double v = WGS84a / Math.Sqrt(1 - m_WGS84eSquared * Math.Pow(Math.Sin(phi), 2));
          double inCoOrdX = (v + ElipsoidHeight) * Math.Cos(phi) * Math.Cos(lamda);
          double inCoOrdY = (v + ElipsoidHeight) * Math.Cos(phi) * Math.Sin(lamda);
          double inCoOrdZ = ((1 - m_WGS84eSquared) * v + ElipsoidHeight) * Math.Sin(phi);

          double prod1 = (1 + scalechange) * inCoOrdX +
              (-1 * rz * 0.000004848136811) * inCoOrdY +
              (ry * 0.000004848136811) * inCoOrdZ +
              -446.448;

          double prod2 = (rz * 0.000004848136811) * inCoOrdX +
              (1 + scalechange) * inCoOrdY +
              (-1 * rx * 0.000004848136811) * inCoOrdZ +
              125.157;

          double prod3 = (-1 * ry * 0.000004848136811) * inCoOrdX +
              (rx * 0.000004848136811) * inCoOrdY +
              (1 + scalechange) * inCoOrdZ +
              -542.06;

          // This 3-d to lat long uses OSGB m_eSquared, NGa

          double p = Math.Sqrt(prod1 * prod1 + prod2 * prod2);
          double lat1 = Math.Atan2(prod3, p * (1 - m_eSquared));
          double v1 = NGa / Math.Sqrt(1 - m_eSquared * Math.Pow(Math.Sin(lat1), 2));
          double lat2 = Math.Atan2(prod3 + m_eSquared * v1 * Math.Sin(lat1), p);
          double v2 = NGa / Math.Sqrt(1 - m_eSquared * Math.Pow(Math.Sin(lat2), 2));

          phi = Math.Atan2(prod3 + m_eSquared * v2 * Math.Sin(lat2), p);
          lamda = Math.Atan2(prod2, prod1);
          ElipsoidHeight = p / Math.Cos(phi) - v2;
       }

       private void DoCalc(bool bApplyOSGB1936Transform, double phi, double lamda, double ElipsoidHeight)
       {
          if (bApplyOSGB1936Transform)
             HelTrans(ref phi, ref lamda, ref ElipsoidHeight);

          double phiSine = Math.Sin(phi);
          double phiSineSquared = phiSine * phiSine;
          double phiCos = Math.Cos(phi);
          double phiCosCubed = phiCos * phiCos * phiCos;
          double phiCosToTheFifth = phiCos * phiCos * phiCosCubed; // ****************************** FIXED
          double phiTan = Math.Tan(phi);
          double phiTanSquared = phiTan * phiTan;
          double phiTanToTheFourth = phiTanSquared * phiTanSquared;
          double OneMinuseSquaredphiSineSquared = 1.0 - m_eSquared * phiSineSquared;
          double phiMinusNGphiOrigin = phi - NGphiOrigin;
          double phiPlusNGphiOrigin = phi + NGphiOrigin;

          double v = m_NGaNGFo / Math.Sqrt(OneMinuseSquaredphiSineSquared);
          double p = m_NGaNGFo * (1.0 - m_eSquared) * Math.Pow((OneMinuseSquaredphiSineSquared), -1.5);
          double nnSquared = v / p - 1.0;

          double frag1 = (1.0 + m_n + (5.0 / 4.0) * (m_nToTheTwo + m_nToTheThree)) * phiMinusNGphiOrigin;
          double frag2 = (3.0 * (m_n + m_nToTheTwo) + (21.0 / 8.0) * m_nToTheThree) * Math.Sin(phiMinusNGphiOrigin) * Math.Cos(phiPlusNGphiOrigin);
          double frag3 = ((15.0 / 8.0) * (m_nToTheTwo + m_nToTheThree)) * Math.Sin(2.0 * (phiMinusNGphiOrigin)) * Math.Cos(2.0 * (phiPlusNGphiOrigin));
          double frag4 = ((35.0 / 24) * m_nToTheThree) * Math.Sin(3.0 * (phiMinusNGphiOrigin)) * Math.Cos(3.0 * (phiPlusNGphiOrigin));

          double lamdaDiff = lamda - NGlamdaOrigin;
          double lamdaDiffSquared = lamdaDiff * lamdaDiff;
          double lamdaDiffToTheFourth = lamdaDiffSquared * lamdaDiffSquared;

          {
             double I = m_NGbNGFo * (frag1 - frag2 + frag3 - frag4) + NGnOrigin;
             double II = (v / 2.0) * phiSine * phiCos;
             double III = (v / 24.0) * phiSine * phiCosCubed * (5 - phiTanSquared + 9 * nnSquared);
             double IIIA = (v / 720.0) * phiSine * phiCosToTheFifth * (61 - 58 * phiTanSquared + phiTanToTheFourth);

             m_Northing = I + II * lamdaDiffSquared + III * lamdaDiffToTheFourth + IIIA * lamdaDiffToTheFourth * lamdaDiffSquared;
          }
          {
             double IV = v * phiCos;
             double V = (v / 6.0) * phiCosCubed * (v / p - phiTanSquared);
             double VI = (v / 120.0) * phiCosToTheFifth * (5 - 18 * phiTanSquared + phiTanToTheFourth + 14 * nnSquared - (58 * (phiTanSquared * nnSquared)));

             m_Easting = NGeOrigin + IV * lamdaDiff + V * lamdaDiffSquared * lamdaDiff + VI * lamdaDiffToTheFourth * lamdaDiff;
          }
       }

       private void BuildOSGBString()
       {
          int Northing = (int)(m_Northing + 0.5);
          int Easting = (int)(m_Easting + 0.5);

          int osgbEasting = Easting / 100000;
          int osgbNorthing = 14 - (Northing / 100000);

          int iNorth = ((osgbNorthing * 5) / 25) * 5 + 'H';
          if (osgbEasting > 4)
             iNorth++;

          if (iNorth >= 'I')
             iNorth++;

          int iEast = (osgbEasting % 5) + 'A';
          iEast += (osgbNorthing * 5) % 25;
          if (iEast >= 'I')
             iEast++;

          result_ = "";
          result_ += (char)iNorth;
          result_ += (char)iEast;
          result_ += (Easting % 100000).ToString("D5");
          result_ += (Northing % 100000).ToString("D5");
          e = Easting;
          n = Northing;
       }

       public void Convert()
       {
          InputValidation();

          double phi = lati_ * pi / 180.0;
          double lamda = longi_ * pi / 180.0;
          DoCalc(true, phi, lamda, 0.0);
          ValidateOSGB();
          BuildOSGBString();
       }
   }
}
